package org.maxgroundstation.simulation;

import org.maxgroundstation.simulation.service.ServiceCommunication;

/**
 * Simulated MAX robot running the maxkernel software.
 * @author Scott Davis
 * 
 * Copyright (C) 2010  Scott Davis
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
public class MaxRobot implements Runnable {
    
    // Length of update thread sleep time (milliseconds).
    private static final long SLEEP_TIME = 20L;
    
    // Range limits for motors.
    public static final float MAX_FRONT_AXLE_MOTOR_LIMIT = 1F;
    public static final float MIN_FRONT_AXLE_MOTOR_LIMIT = -1F;
    public static final float MAX_REAR_AXLE_MOTOR_LIMIT = 1F;
    public static final float MIN_REAR_AXLE_MOTOR_LIMIT = -1F;
    public static final float MAX_FRONT_WHEELS_TURN_ANGLE_LIMIT = (float) Math.PI * 25F / 180F;
    public static final float MIN_FRONT_WHEELS_TURN_ANGLE_LIMIT = (float) Math.PI * -25F / 180F;
    public static final float MAX_REAR_WHEELS_TURN_ANGLE_LIMIT = (float) Math.PI * 25F / 180F;
    public static final float MIN_REAR_WHEELS_TURN_ANGLE_LIMIT = (float) Math.PI * -25F / 180F;
    //public static final float MAX_HEAD_PAN_ANGLE = 5F * (float) Math.PI / 4F;
    //public static final float MIN_HEAD_PAN_ANGLE = -5F * (float) Math.PI / 4F;
    public static final float MAX_HEAD_TILT_ANGLE = (float) Math.PI / 2F;
    public static final float MIN_HEAD_TILT_ANGLE = (float) Math.PI / -4F;
    
    // Motor change rates per second.
    private static final float FRONT_AXLE_MOTOR_ACCELERATION_RATE = 1F;
    private static final float REAR_AXLE_MOTOR_ACCELERATION_RATE = 1F;
    private static final float FRONT_WHEELS_TURN_RATE = (float) Math.PI / 2F;
    private static final float REAR_WHEELS_TURN_RATE = (float) Math.PI / 2F;
    private static final float HEAD_PAN_RATE = (float) Math.PI / 4F;
    private static final float HEAD_TILT_RATE = (float) Math.PI / 4F;
    
    // Default location (NASA Ames Research Center)
    // (note, these values are replaced by the VideoService when world model data are loaded)
    private static final double DEFAULT_LATITUDE = 37.418368D;
    private static final double DEFAULT_LONGITUDE = -122.06053D;
    
    // Default HDOP (Horizontal Dilution of Precision)
    private static final float DEFAULT_HDOP = 1F;
    
    // Maximum forward/reverse speed (m/s).
    private static final float MAX_SPEED = 5F;
    
    // The distance between the two axles (meters).
    private static final float AXLE_DISTANCE = .38F;
    
    // The approximate length of a degree latitude (km).
    private static final double LATITUDE_DEGREE_SIZE = 111D;
    
    // The approximate length of a degree longitude at the equator (km).
    private static final double LONGITUDE_EQUATOR_DEGREE_SIZE = 111D;
    
    // The default elevation.
    private static final float DEFAULT_ELEVATION = 20F;
    
    // The default number of satellites.
    private static final int DEFAULT_SATELLITES = 10;
    
    // Data members
    private float frontAxleMotor;
    private float frontAxleMotorTarget;
    private float frontWheelsTurnAngle;
    private float frontWheelsTurnAngleTarget;
    private float rearAxleMotor;
    private float rearAxleMotorTarget;
    private float rearWheelsTurnAngle;
    private float rearWheelsTurnAngleTarget;
    private float headPan;
    private float headPanTarget;
    private float headTilt;
    private float headTiltTarget;
    private boolean antiRoll;
    private float wifiSignalStrength;
    private double longitude;
    private double latitude;
    private float travelHeading;
    private float speed;
    private float hdop;
    private float elevation;
    private int satellites;
    private float facing;
    private boolean run;
    private Thread updateThread;
    private UdpCommandReceiver udpReceiver;
    private ServiceCommunication serviceComm;
    
    /**
     * Constructor
     */
    public MaxRobot() {
        // Initialize data members.
        frontAxleMotor = 0F;
        frontAxleMotorTarget = 0F;
        frontWheelsTurnAngle = 0F;
        frontWheelsTurnAngleTarget = 0F;
        rearAxleMotor = 0F;
        rearAxleMotorTarget = 0F;
        rearWheelsTurnAngle = 0F;
        rearWheelsTurnAngleTarget = 0F;
        headPan = 0F;
        headPanTarget = 0F;
        headTilt = 0F;
        headTiltTarget = 0F;
        antiRoll = false;
        wifiSignalStrength = 100F;
        longitude = DEFAULT_LONGITUDE;
        latitude = DEFAULT_LATITUDE;
        travelHeading = 0F;
        speed = 0F;
        hdop = DEFAULT_HDOP;
        facing = 0F;
        elevation = DEFAULT_ELEVATION;
        satellites = DEFAULT_SATELLITES;
        
        udpReceiver = new UdpCommandReceiver(this);
        serviceComm = new ServiceCommunication(this);
        
        // Start the update thread.
        start();
    }
    
    /**
     * Sets the update thread to run or not.
     * @param run true if robot is to be updated.
     */
    public void setRun(boolean run) {
        this.run = run;
    }
    
    /**
     * Starts the update thread.
     */
    private void start() {
        if ((updateThread == null) || !run) {
            updateThread = new Thread(this, "robot update thread");
            updateThread.start();
        }
    }
    
    /**
     * Run the update thread.
     */
    public void run() {
    	run = true;
        while (run) {
            try {
                Thread.sleep(SLEEP_TIME);
            } 
            catch (InterruptedException e) {}
            
            // Update all the motors.
            updateMotors();
            
            // Update robot movement.
            updateMovement();
        }
    }
    
    /**
     * Destroy the robot sim.
     */
    public void destroy() {
        run = false;    
        udpReceiver.closeSocket();
        serviceComm.destroy();
    }
    
    /**
     * Gets the motor power of the front axle.
     * Positive value means forward rotation.
     * Negative value means reverse rotation.
     * @return motor power (-1 to 1).
     */
    public float getFrontAxleMotor() {
        return frontAxleMotor;
    }
    
    /**
     * Sets the front axle motor power.
     * Positive motor value means forward rotation.
     * Negative motor value means reverse rotation.
     * @param motor the motor power (-1 to 1).
     * @throws Exception if motor value is out of range.
     */
    void setFrontAxleMotor(float motor) throws Exception {
        if (motor > MAX_FRONT_AXLE_MOTOR_LIMIT) {
            throw new Exception("motor: " + motor + " exceeds front axle motor limit of " + 
                    MAX_FRONT_AXLE_MOTOR_LIMIT);
        }
        else if (motor < MIN_FRONT_AXLE_MOTOR_LIMIT) {
            throw new Exception("motor: " + motor + " exceeds front axle motor limit of " + 
                    MIN_FRONT_AXLE_MOTOR_LIMIT);
        }
        else {
            frontAxleMotorTarget = motor;
        }
    }
    
    /**
     * Gets the turn angle of the front wheels.
     * Negative angle value is counter-clockwise.
     * Zero angle value is straight.
     * Positive angle value is clockwise;
     * @return rotation angle in radians.
     */
    public float getFrontWheelsTurnAngle() {
        return frontWheelsTurnAngle;
    }
    
    /**
     * Sets the turn angle of the front wheels.
     * Negative angle value is counter-clockwise.
     * Zero angle is straight.
     * Positive angle value is clockwise.
     * @param angle rotation angle in radians.
     * @throws Exception if angle exceeds wheels' turn angle limits.
     */
    void setFrontWheelsTurnAngle(float angle) throws Exception {
        if (angle > MAX_FRONT_WHEELS_TURN_ANGLE_LIMIT) {
            throw new Exception("angle: " + angle + " exceeds front wheels turn angle limit of " + 
                    MAX_FRONT_WHEELS_TURN_ANGLE_LIMIT + " radians.");
        }
        else if (angle < MIN_FRONT_WHEELS_TURN_ANGLE_LIMIT) {
            throw new Exception("angle: " + angle + " exceeds front wheels turn angle limit of " + 
                    MAX_FRONT_WHEELS_TURN_ANGLE_LIMIT + " radians.");
        }
        else {
            frontWheelsTurnAngleTarget = angle;
            antiRoll = false;
        }
    }
    
    /**
     * Gets the motor power of the rear axle.
     * Positive motor value means forward rotation.
     * Negative motor value means reverse rotation.
     * @return motor power (-1 to 1).
     */
    public float getRearAxleMotor() {
        return rearAxleMotor;
    }
    
    /**
     * Sets the rear axle motor power.
     * Positive motor value means forward rotation.
     * Negative motor value means reverse rotation.
     * @param motor the motor power (-1 to 1).
     * @throws Exception if motor value is out of range.
     */
    void setRearAxleMotor(float motor) throws Exception {
        if (motor > MAX_REAR_AXLE_MOTOR_LIMIT) {
            throw new Exception("motor: " + motor + " exceeds rear axle motor limit of " + 
                    MAX_REAR_AXLE_MOTOR_LIMIT);
        }
        else if (motor < MIN_REAR_AXLE_MOTOR_LIMIT) {
            throw new Exception("motor: " + motor + " exceeds rear axle motor limit of " + 
                    MIN_REAR_AXLE_MOTOR_LIMIT);
        }
        else {
            rearAxleMotorTarget = motor;
        }
    }
    
    /**
     * Gets the turn angle of the rear wheels.
     * Negative angle value is counter-clockwise.
     * Zero angle value is straight.
     * Positive angle value is clockwise;
     * @return rotation angle in radians.
     */
    public float getRearWheelsTurnAngle() {
        return rearWheelsTurnAngle;
    }
    
    /**
     * Sets the turn angle of the rear wheels.
     * Negative angle value is counter-clockwise.
     * Zero angle is straight.
     * Positive angle value is clockwise.
     * @param angle rotation angle in radians.
     * @throws Exception if angle exceeds wheels' turn angle limits.
     */
    void setRearWheelsTurnAngle(float angle) throws Exception {
        if (angle > MAX_REAR_WHEELS_TURN_ANGLE_LIMIT) {
            throw new Exception("angle: " + angle + " exceeds rear wheels turn angle limit of " + 
                    MAX_REAR_WHEELS_TURN_ANGLE_LIMIT + " radians.");
        }
        else if (angle < MIN_REAR_WHEELS_TURN_ANGLE_LIMIT) {
            throw new Exception("angle: " + angle + " exceeds rear wheels turn angle limit of " + 
                    MAX_REAR_WHEELS_TURN_ANGLE_LIMIT + " radians.");
        }
        else {
            rearWheelsTurnAngleTarget = angle;
            antiRoll = false;
        }
    }
    
    /**
     * Sets the turn angle of the front and rear wheels with anti-roll feature turned on.
     * @param angle rotation angle in radians.
     * @throws Exception if angle exceeds wheels' turn angle limits.
     */
    void setAntiRollWheelsTurnAngle(float angle) throws Exception {
        if (angle > MAX_FRONT_WHEELS_TURN_ANGLE_LIMIT) {
            throw new Exception("angle: " + angle + " exceeds front wheels turn angle limit of " + 
                    MAX_FRONT_WHEELS_TURN_ANGLE_LIMIT + " radians.");
        }
        else if (angle < MIN_FRONT_WHEELS_TURN_ANGLE_LIMIT) {
            throw new Exception("angle: " + angle + " exceeds front wheels turn angle limit of " + 
                    MAX_FRONT_WHEELS_TURN_ANGLE_LIMIT + " radians.");
        }
        if (angle < (0F - MAX_REAR_WHEELS_TURN_ANGLE_LIMIT)) {
            throw new Exception("angle: " + angle + " exceeds rear wheels turn angle limit of " + 
                    MAX_REAR_WHEELS_TURN_ANGLE_LIMIT + " radians.");
        }
        else if (angle > (0F - MIN_REAR_WHEELS_TURN_ANGLE_LIMIT)) {
            throw new Exception("angle: " + angle + " exceeds rear wheels turn angle limit of " + 
                    MAX_REAR_WHEELS_TURN_ANGLE_LIMIT + " radians.");
        }
        else {
            System.out.println("turn angle: " + angle);
            antiRoll = true;
            frontWheelsTurnAngleTarget = angle;
            rearWheelsTurnAngleTarget = 0F - angle;
        }
    }
    
    /**
     * Gets the pan angle of the head turret.
     * Negative angle value is counter-clockwise.
     * Zero angle value is straight ahead.
     * Positive angle value is clockwise.
     * @return pan angle in radians.
     */
    public float getHeadPanAngle() {
        return headPan;
    }
    
    /**
     * Sets the pan angle of the head turret.
     * Negative angle value is counter-clockwise.
     * Zero angle value is straight ahead.
     * Positive angle value is clockwise.
     * @param angle the pan angle in radians.
     * @throws Exception if pan angle exceeds head pan range.
     */
    void setHeadPanAngle(float angle) throws Exception {
    	/*
        if (angle > MAX_HEAD_PAN_ANGLE) {
            throw new Exception("angle: " + angle + " exceeds head pan angle limit of " + 
                    MAX_HEAD_PAN_ANGLE + " radians.");
        }
        else if (angle < MIN_HEAD_PAN_ANGLE) {
            throw new Exception("turn angle: " + angle + " exceeds head pan angle limit of " + 
                    MIN_HEAD_PAN_ANGLE + " radians.");
        }
        else {
        */
        headPanTarget = panAngleCorrection(angle);
            /*
        }
        */
    }
    
    /**
     * Gets the tilt angle of the head turret.
     * Negative angle value is downwards from level.
     * Zero angle value is level.
     * Positive angle value is upwards from level.
     * @return tilt angle in radians.
     */
    public float getHeadTiltAngle() {
        return headTilt;
    }
    
    /**
     * Sets the tile angle of the head turret.
     * Negative angle value is downwards from level.
     * Zero angle value is level.
     * Positive angle value is upwards from level.
     * @param tiltAngle the tilt angle in radians.
     * @throws Exception if tilt angle exceeds head tilt range.
     */
    void setHeadTiltAngle(float tiltAngle) throws Exception {
        if (tiltAngle > MAX_HEAD_TILT_ANGLE) {
            throw new Exception("tilt angle: " + tiltAngle + " exceeds head tilt angle limit of " + 
                    MAX_HEAD_TILT_ANGLE + " radians.");
        }
        else if (tiltAngle < MIN_HEAD_TILT_ANGLE) {
            throw new Exception("tilt angle: " + tiltAngle + " exceeds head tilt angle limit of " + 
                    MIN_HEAD_TILT_ANGLE + " radians.");
        }
        else {
            headTiltTarget = tiltAngle;
        }
    }
    
    /**
     * Gets the robot's WiFi signal strength.
     * @return percentage signal strength.
     */
    public float getWiFiSignalStrength() {
        return wifiSignalStrength;
    }
    
    /**
     * Sets the robot's WiFi signal strength.
     * @param wifiSignalStrength the WiFi signal strength.
     */
    public void setWiFiSignalStrength(float wifiSignalStrength) {
        this.wifiSignalStrength = wifiSignalStrength;
    }
    
    /**
     * Gets the robot's latitude position.
     * Negative value represents degrees south.
     * @return latitude (degrees).
     */
    public double getLatitude() {
        return latitude;
    }
    
    /**
     * Sets the robot's latitude position.
     * Negative value represents degrees south.
     * @param latitude the new latitude (degrees).
     */
    public void setLatitude(double latitude) {
        this.latitude = latitude;
    }
    
    /**
     * Gets the robot's longitude position.
     * Range: -180 (west) to +180 (east)
     * @return longitude (degrees).
     */
    public double getLongitude() {
        return longitude;
    }
    
    /**
     * Sets the robot's longitude position.
     * Range: -180 (west) to +180 (east)
     * @param longitude the new longitude (degrees).
     */
    public void setLongitude(double longitude) {
        this.longitude = longitude;
    }
    
    /**
     * Gets the robot's facing direction.
     * @return facing direction (radians clockwise from North).
     */
    public float getFacing() {
        return facing;
    }
    
    /**
     * Sets the robot's facing direction.
     * @param facing the facing direction (radians clockwise from North).
     */
    public void setFacing(float facing) {
        this.facing = facing;
    }
    
    /**
     * Gets the robot's direction of travel.
     * @return travel heading (radians clockwise from North).
     */
    public float getTravelHeading() {
        return travelHeading;
    }
    
    /**
     * Sets the robot's direction of travel.
     * @param travelHeading (radians clockwise from North).
     */
    public void setTravelHeading(float travelHeading) {
        this.travelHeading = travelHeading;
    }
    
    /**
     * Gets the robot's speed.
     * @return speed (m/s)
     */
    public float getSpeed() {
        return speed;
    }
    
    /**
     * Sets the robot's speed.
     * @param speed the new speed (m/s).
     */
    public void setSpeed(float speed) {
        this.speed = speed;
    }
    
    /**
     * Gets the robot's HDOP (Horizontal Dilution of Precision)
     * @return HDOP value.
     */
    public float getHdop() {
        return hdop;
    }
    
    /**
     * Sets the robot's HDOP (Horizontal Dilution of Precision)
     * @param hdop the new HDOP value.
     */
    public void setHdop(float hdop) {
        this.hdop = hdop;
    }
    
    /**
     * Gets the robot's elevation.
     * @return elevation (meters above sea level).
     */
    public float getElevation() {
        return elevation;
    }
    
    /**
     * Sets the robot's elevation.
     * @param elevation the elevation (meters above sea level).
     */
    public void setElevation(float elevation) {
        this.elevation = elevation;
    }
    
    /**
     * Gets the number of satellites.
     * @return number of satellites.
     */
    public int getSatellites() {
        return satellites;
    }
    
    /**
     * Sets the number of satellites.
     * @param satellites number of satellites.
     */
    public void setSatellites(int satellites) {
        this.satellites = satellites;
    }
    
    /**
     * Sends a formatted command to the robot.
     * @param commandString the command string.
     * @return result code (see CommandParser).
     */
    public int sendCommand(String commandString) {
        return CommandParser.parseCommand(this, commandString);
    }
    
    /**
     * Sends a formatted text command to the robot.
     * @param commandText the command text.
     * @return result code (see CommandParser).
     */
    public int sendTextCommand(String commandText) {
        return CommandParser.parseTextCommand(this, commandText);
    }
    
    /**
     * Update all the motors in the robot.
     */
    private void updateMotors() {
        float secondsTime = (float) SLEEP_TIME / 1000F;
        
        // Update front axle motor.
        if (frontAxleMotorTarget != frontAxleMotor) {
            float accelerationChange = FRONT_AXLE_MOTOR_ACCELERATION_RATE * secondsTime;
            if (frontAxleMotor < frontAxleMotorTarget) {
                frontAxleMotor += accelerationChange;
                if (frontAxleMotor > frontAxleMotorTarget) frontAxleMotor = frontAxleMotorTarget;
            }
            else {
                frontAxleMotor -= accelerationChange;
                if (frontAxleMotor < frontAxleMotorTarget) frontAxleMotor = frontAxleMotorTarget;
            }
        }
        
        // Update rear axle motor.
        if (rearAxleMotorTarget != rearAxleMotor) {
            float accelerationChange = REAR_AXLE_MOTOR_ACCELERATION_RATE * secondsTime;
            if (rearAxleMotor < rearAxleMotorTarget) {
                rearAxleMotor += accelerationChange;
                if (rearAxleMotor > rearAxleMotorTarget) rearAxleMotor = rearAxleMotorTarget;
            }
            else {
                rearAxleMotor -= accelerationChange;
                if (rearAxleMotor < rearAxleMotorTarget) rearAxleMotor = rearAxleMotorTarget;
            }
        }
        
        // Update front wheels turn angle.
        float currentFrontWheelsTurnAngleTarget = frontWheelsTurnAngleTarget;
        if (antiRoll) currentFrontWheelsTurnAngleTarget = 
            determineAntiRollTurnAngleTarget(frontWheelsTurnAngleTarget, true);
        if (currentFrontWheelsTurnAngleTarget != frontWheelsTurnAngle) {
            float angleChange = FRONT_WHEELS_TURN_RATE * secondsTime;
            if (frontWheelsTurnAngle < currentFrontWheelsTurnAngleTarget) {
                frontWheelsTurnAngle += angleChange;
                if (frontWheelsTurnAngle > currentFrontWheelsTurnAngleTarget) 
                    frontWheelsTurnAngle = currentFrontWheelsTurnAngleTarget;
            }
            else {
                frontWheelsTurnAngle -= angleChange;
                if (frontWheelsTurnAngle < currentFrontWheelsTurnAngleTarget) 
                    frontWheelsTurnAngle = currentFrontWheelsTurnAngleTarget;
            }
        }
        
        // Update rear wheels turn angle.
        float currentRearWheelsTurnAngleTarget = rearWheelsTurnAngleTarget;
        if (antiRoll) currentRearWheelsTurnAngleTarget = 
            determineAntiRollTurnAngleTarget(rearWheelsTurnAngleTarget, false);
        if (currentRearWheelsTurnAngleTarget != rearWheelsTurnAngle) {
            float angleChange = REAR_WHEELS_TURN_RATE * secondsTime;
            if (rearWheelsTurnAngle < currentRearWheelsTurnAngleTarget) {
                rearWheelsTurnAngle += angleChange;
                if (rearWheelsTurnAngle > currentRearWheelsTurnAngleTarget) 
                    rearWheelsTurnAngle = currentRearWheelsTurnAngleTarget;
            }
            else {
                rearWheelsTurnAngle -= angleChange;
                if (rearWheelsTurnAngle < currentRearWheelsTurnAngleTarget) 
                    rearWheelsTurnAngle = currentRearWheelsTurnAngleTarget;
            }
        }
        
        // Update head pan angle.
        if (headPanTarget != headPan) {
            float angleChange = HEAD_PAN_RATE * secondsTime;
            if ((Math.abs(headPanTarget - headPan) < angleChange) || 
                    ((Math.PI * 2D) - Math.abs(headPanTarget - headPan) < angleChange)) {
                headPan = headPanTarget;
            }
            else {
                if (Math.abs(headPanTarget - headPan) < Math.PI) {
                    if (headPanTarget < headPan) {
                        headPan = panAngleCorrection(headPan - angleChange);
                    }
                    else {
                        headPan = panAngleCorrection(headPan + angleChange);
                    }
                }
                else {
                    if (headPanTarget < headPan) {
                        headPan = panAngleCorrection(headPan + angleChange);
                    }
                    else {
                        headPan = panAngleCorrection(headPan - angleChange);
                    }
                }
            }
        }
        
        // Update head tilt angle.
        if (headTiltTarget != headTilt) {
            float angleChange = HEAD_TILT_RATE * secondsTime;
            if (headTilt < headTiltTarget) {
                headTilt += angleChange;
                if (headTilt > headTiltTarget) headTilt = headTiltTarget;
            }
            else {
                headTilt -= angleChange;
                if (headTilt < headTiltTarget) headTilt = headTiltTarget;
            }
        }
    }
    
    private float panAngleCorrection(float angle) {
        if (angle < 0F) {
            angle += ((float) Math.PI * 2F);
        }
        if (angle > ((float) Math.PI * 2F)) {
            angle -= ((float) Math.PI * 2F);
        }
        return angle;
    }
    
    /**
     * Determines the turn angle target to use for anti-roll feature.
     * @param turnAngleTarget the actual turn angle target.
     * @param frontWheels true if front wheels, false if rear wheels.
     * @return anti-roll turn angle target.
     */
    private float determineAntiRollTurnAngleTarget(float turnAngleTarget, boolean frontWheels) {
        float result = turnAngleTarget;
        
        // Determine turn angle ratio.
        float turnRatio = 0F;
        if (frontWheels) {
            if (turnAngleTarget >= 0F) turnRatio = turnAngleTarget / MAX_FRONT_WHEELS_TURN_ANGLE_LIMIT;
            else turnRatio = turnAngleTarget / MIN_FRONT_WHEELS_TURN_ANGLE_LIMIT;
        }
        else {
            if (turnAngleTarget >= 0F) turnRatio = turnAngleTarget / MAX_REAR_WHEELS_TURN_ANGLE_LIMIT;
            else turnRatio = turnAngleTarget / MIN_REAR_WHEELS_TURN_ANGLE_LIMIT;
        }
        
        // Determine speed ratio.
        float speedRatio = 0F;
        if (frontWheels) {
            if (frontAxleMotorTarget >= 0F) speedRatio = frontAxleMotorTarget / MAX_FRONT_AXLE_MOTOR_LIMIT;
            else speedRatio = frontAxleMotorTarget / MIN_FRONT_AXLE_MOTOR_LIMIT;
        }
        else {
            if (rearAxleMotorTarget >= 0F) speedRatio = rearAxleMotorTarget / MAX_REAR_AXLE_MOTOR_LIMIT;
            else speedRatio = rearAxleMotorTarget / MIN_REAR_AXLE_MOTOR_LIMIT;
        }
        
        // Adjust turn angle based on speed.
        if (speedRatio > .5F) {
            float turnRatioLimit = 1.5F - speedRatio;
            if (turnRatio > turnRatioLimit) result = (turnRatioLimit / turnRatio) * turnAngleTarget;
        }
        
        return result;
    }
    
    /**
     * Update the robot's position and facing.
     */
    private void updateMovement() {
        float secondsTime = (float) SLEEP_TIME / 1000F;
        
        // Update speed.
        speed = getFrontAxleMotor() * MAX_SPEED;
        
        // Determine distance traveled.
        float distanceTraveled = Math.abs(speed) * secondsTime;
        
        // Update facing.
        float initialFacing = facing;
        float totalTurn = (getFrontWheelsTurnAngle() / 2F) - (getRearWheelsTurnAngle() / 2F);
        float facingChange = 0F;
        if (totalTurn != 0F) {
            float radius = AXLE_DISTANCE / totalTurn;
            facingChange = distanceTraveled / radius;
            if (speed < 0F) facingChange *= -1F;
            facing += facingChange;
            
            // Make sure facing value is between 0 and 2 PI.
            if (facing > (Math.PI * 2F)) {
                facing -= (Math.PI * 2F);
            }
            if (facing < 0F) {
                facing += (Math.PI * 2F);
            }
        }
        
        // Determine crabbing angle.
        float crabbingAngle = 0F;
        if ((getFrontWheelsTurnAngle() > 0F) && (getRearWheelsTurnAngle() > 0F)) {
            crabbingAngle = Math.min(getFrontWheelsTurnAngle(), getRearWheelsTurnAngle());
        }
        else if ((getFrontWheelsTurnAngle() < 0F) && (getRearWheelsTurnAngle() < 0F)) {
            crabbingAngle = Math.max(getFrontWheelsTurnAngle(), getRearWheelsTurnAngle());
        }
        
        // Update direction of travel.
        if (speed > 0F) {
            travelHeading = initialFacing + (facingChange / 2F) + crabbingAngle;
        }
        else if (speed < 0F) {
            travelHeading = initialFacing + (facingChange / 2F) + crabbingAngle + (float) Math.PI;
        }
        
        // Make sure travel heading is between 0 and 2 PI.
        if (travelHeading > (Math.PI * 2F)) {
            travelHeading -= (Math.PI * 2F);
        }
        if (travelHeading < 0F) {
            travelHeading += (Math.PI * 2F);
        }
        
        // Update position.
        // xDiff is positive for east, negative for west.
        float xDiff = (float) Math.sin(travelHeading) * distanceTraveled;
        // yDiff is positive for north, negative for south.
        float yDiff = (float) Math.cos(travelHeading) * distanceTraveled;
        
        double latDiff = yDiff / (LATITUDE_DEGREE_SIZE * 1000D);
        
        // Must determine longitude degree length (km) at current latitude.
        double latitudeRad = Math.abs(latitude) * Math.PI / 180D;
        double longitudeLength = Math.cos(latitudeRad) * LONGITUDE_EQUATOR_DEGREE_SIZE;
        double longDiff = 0D;
        if (longitudeLength > 0D) longDiff = xDiff / (longitudeLength * 1000D);
        
        // Update latitude.
        latitude += latDiff;
        
        // Make sure latitude is in valid range (-90 to 90).
        if (latitude > 90D) {
            latitude -= 90D;
            longitude += 180D;
        }
        if (latitude < -90D) {
            latitude += 90D;
            longitude += 180D;
        }
        
        // Update longitude.
        longitude += longDiff;
        
        // Make sure longitude is within valid range (-180 to 180).
        if (longitude < -180D) {
            longitude += 360D;
        }
        if (longitude > 180D) {
            longitude -= 360D;
        }
    }
    
    /**
     * Gets the UDP command receiver for the robot.
     * @return UDP command receiver.
     */
    UdpCommandReceiver getUdpCommandReceiver() {
        return udpReceiver;
    }
    
    /**
     * Gets the service communication for the robot.
     * @return service communication.
     */
    ServiceCommunication getServiceCommunication() {
    	return serviceComm;
    }
    
    /**
     * Gets the MAX robot simulation's local host IP address.
     * @return host IP address.
     */
    public String getHostIP() {
        return udpReceiver.getHostIP().getHostAddress();
    }
    
    /**
     * Gets the MAX robot simulation's local port.
     * @return port number.
     */
    public int getPort() {
        return udpReceiver.getPort();
    }
}